#include <cmath>
#include <vector>

#include "mathmatics.hpp"
#include "predefined.hpp"
#include "global.hpp"

using namespace std;

Mathmatics::Mathmatics()
{
}

Mathmatics::~Mathmatics()
{
}

vector<double> Mathmatics::matrixVectorMultiply(const vector<vector<double>> &A, const vector<double> &x)
{
    size_t rows = A.size();
    size_t cols = A[0].size();
    // Initialize the result vector with zeros
    vector<double> returnResult(rows, 0.0);

    // Check if the number of columns in the matrix matches the number of elements in the vector
    if (cols != x.size()) {
        cerr << "Error: Matrix columns and vector size do not match!" << endl;
        return {};
    }

    // Perform the multiplication
    for (size_t i = 0; i < rows; ++i) {
        for (size_t j = 0; j < cols; ++j) {
            returnResult[i] += A[i][j] * x[j];
        }
    }

    return returnResult;
}

vector<double> Mathmatics::findRootsofJnPrime(int n, int num_roots) {
    vector<double> roots;
    double x_min = 0; // Start searching from x = 0.1
    double x_max = x_min;

    while (roots.size() < static_cast<long unsigned int> (num_roots)) {
        x_max += m_stepSize; // Expand the range

        double x = x_min;
        while (x + m_stepSize <= x_max && roots.size() < static_cast<long unsigned int> (num_roots)) {
            double x_lower = x;
            double x_upper = x + m_stepSize;

            double f_lower = m_besselJ_derivative(n, x_lower);
            double f_upper = m_besselJ_derivative(n, x_upper);

            // Check if the derivative changes sign in this interval
            if (f_lower * f_upper < 0) {
                double root = m_bisection(n, x_lower, x_upper);
                if (!isnan(root)) {
                    roots.push_back(root);
                }
            }

            // Move to the next interval
            x += m_stepSize;
        }

        // Update x_min for the next iteration
        x_min = x_max;
    }

    return roots;
}

// Function to compute the first derivative of the Bessel function J_n(x)
double Mathmatics::m_besselJ_derivative(int n, double x) {
    if (n == 0) {
        // Special case for n = 0: J_0'(x) = -J_1(x)
        return -cyl_bessel_j(1, x);
    } else {
        // General case for n >= 1: J_n'(x) = (J_{n-1}(x) - J_{n+1}(x)) / 2
        return (cyl_bessel_j(n - 1, x) - cyl_bessel_j(n + 1, x)) / 2.0;
    }
}

double Mathmatics::m_bisection(int n, double a, double b) {
    double fa = m_besselJ_derivative(n, a);
    double fb = m_besselJ_derivative(n, b);

    if (fa * fb >= 0) {
        return NAN; // No root in this interval
    }

    double c = a;
    for (int i = 0; i < m_maxIter; ++i) {
        c = (a + b) / 2.0;
        double fc = m_besselJ_derivative(n, c);

        if (fabs(fc) < TOLERAN || (b - a) / 2.0 < TOLERAN) {
            return c; // Root found
        }

        if (fc * fa < 0) {
            b = c;
            fb = fc;
        } else {
            a = c;
            fa = fc;
        }
    }

    return c; // Approximate root
}



double Mathmatics::max(double a, double b){
    return (a>b)?a:b;
}

int Mathmatics::max(int a, int b){
    return (a>b)?a:b;
}

double Mathmatics::min(double a, double b){
    return (a<b)?a:b;
}

int Mathmatics::min(int a, int b){
    return (a<b)?a:b;
}

double Mathmatics::determinant(double *Mat,int order){
    double det=0,*subMat;
    int iCol,isMov=0,iMatRow,iSubMatRow,flag;
    //printf("order: %d\n",order);
    //printf("order: %d,Mat[0]: %lf\n",order,Mat[0]);

    if (order==1)return Mat[0];
    subMat=(double*)malloc((order-1)*(order-1)*sizeof(double));

    for (iMatRow=0;iMatRow<order;++iMatRow){
        for (iSubMatRow=0;iSubMatRow<order-1;++iSubMatRow){
            isMov=iMatRow>iSubMatRow?0:1;
            for (iCol=0;iCol<order-1;iCol++){
                subMat[iSubMatRow*(order-1)+iCol]=Mat[(iSubMatRow+isMov)*order+iCol+1];
            }
        }
        flag=(iMatRow%2==0?1:-1);
        //printf("order: %d,det: %lf",order,det);
        det+=flag*(*(Mat+iMatRow*order))*determinant(subMat,order-1);

    }
    //free(subMat);
    return det;
}

void Mathmatics::CartisianToPolar(double *xyz, double *rtp,double *uVecR, double *uVecTheta) {
    double xyz2[NDIM];
    for (int i = 0; i < NDIM; ++i) {
        xyz2[i] = xyz[i]-inputCtrl.srcPointPos[i]; // Adjust for polar origin
    }
    rtp[0] = sqrt(xyz2[0] * xyz2[0] + xyz2[1] * xyz2[1]); // r
    rtp[1] = atan2(xyz2[1], xyz2[0]); // theta
    if (rtp[1] < 0) {
        rtp[1] = 2*pi+ rtp[1]; // Ensure theta is in the range [0, 2π)
    }
    uVecR[0]=cos(rtp[1]);
    uVecR[1]=sin(rtp[1]);
    uVecTheta[0]=-sin(rtp[1]);
    uVecTheta[1]=cos(rtp[1]);
}

void Mathmatics::CartisianToCylindrical(double *xyz, double *rtp,double *uVecR, double *uVecTheta) {
    double xyz2[MAXDIM];
    for (int i = 0; i < MAXDIM; ++i) {
        xyz2[i] = xyz[i]-inputCtrl.srcPointPos[i]; //Adjust for cylindrical origin
    }
    rtp[0] = sqrt(xyz2[0] * xyz2[0] + xyz2[1] * xyz2[1]); // r
    rtp[1] = atan2(xyz2[1], xyz2[0]); // theta
    rtp[2] = xyz2[2]; // z (cylindrical coordinate)
    uVecR[0]=cos(rtp[1]);
    uVecR[1]=sin(rtp[1]);
    uVecR[2]=0.0; // In cylindrical coordinates, r does not change the z-component
    uVecTheta[0]=-sin(rtp[1]);
    uVecTheta[1]=cos(rtp[1]);
    uVecTheta[2]=0.0; // In cylindrical coordinates, theta does not change the z-component
}

void Mathmatics::CartisianToSpherical(double *xyz, double *rtp,double *uVecR, double *uVecTheta, double *uVecPhi) {
    double xyz2[MAXDIM];
    for (int i = 0; i < MAXDIM; ++i) {
        xyz2[i] = xyz[i]-inputCtrl.srcPointPos[i]; // Adjust for spherical origin
    }
    rtp[0] = sqrt(xyz2[0] * xyz2[0] + xyz2[1] * xyz2[1] + xyz2[2] * xyz2[2]); // r
    rtp[1] = atan2(sqrt(xyz2[0] * xyz2[0] + xyz2[1] * xyz2[1]), xyz2[2]); // theta
    rtp[2] = atan2(xyz2[1], xyz2[0]); // phi
    uVecR[0] = sin(rtp[1])*cos(rtp[2]);
    uVecR[1] = sin(rtp[1])*sin(rtp[2]);
    uVecR[2] = cos(rtp[1]);
    uVecTheta[0] = cos(rtp[1])*cos(rtp[2]);
    uVecTheta[1] = cos(rtp[1])*sin(rtp[2]);
    uVecTheta[2] = -sin(rtp[1]);
    uVecPhi[0] = -sin(rtp[2]);
    uVecPhi[1] = cos(rtp[2]);
    uVecPhi[2] = 0.0; // In spherical coordinates, phi does not change the z-component
}

double Mathmatics::vectorDotProduct(int N, double *a, double *b) {
    double result = 0.0;
    for (int i = 0; i < N; ++i) {
        result += a[i] * b[i];
    }
    return result;
}